#include "ch_serial.h"

#if defined(CH_DEBUG)
#include <stdio.h>
#define CH_TRACE	printf
#else
#define CH_TRACE(...)
#endif

//// ID20 状态数据包
//#define ID20         (0x14)        /* 数据包ID */
#define LEN20         (0x64)        /* 数据包长度 */

//// ID28 原始数据包
//#define ID28         (0x1C)        /* 数据包ID */
#define LEN28         (0x30)        /* 数据包长度 */

//// ID31 卫星信息数据
//#define ID31         (0x1F)        /* 数据包ID */
//#define LEN31         (0x7E)        /* 数据包长度 */

#define CH_HDR_SIZE     (0x05)        /* CHAOHE protocol header size */
#define LRC_SIZE        (0x01)        /* LRC 数据头的字节数*/

/* common type conversion */
#define U1(p) (*((uint8_t *)(p)))
#define I1(p) (*((int8_t  *)(p)))
#define I2(p) (*((int16_t  *)(p)))
static uint16_t U2(uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;}
static uint32_t U4(uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;}
static float    R4(uint8_t *p) {float    r; memcpy(&r,p,4); return r;}
static double   R8(uint8_t *p) {double   r; memcpy(&r,p,8); return r;}

/**
 * @brief LRC 实现函数  LRC = ((packet_id + packet_length + crc[0] + crc[1])^0xFF) + 1
 * @param currect_crc previous crc value, set 0 if it's first section
 * @param src   source stream data
 * @param len
 * */
static void lrc_check(uint8_t *currect_lrc,const uint8_t *src, uint32_t len){
    uint8_t lrc=0;
    for (uint8_t i=0; i < len; ++i){
        lrc += src[i];
    }
    lrc ^=0xFF;
    *currect_lrc = lrc+1;
}

/* dump hi229 or hi226 data packet*/
/**
 * @brief 用于Debug
 * */
void ch_dump_imu_data(raw_t *raw)
{
    int i;
    if(raw->item_code[0] != ID20)
    {
        CH_TRACE("%-16s%d\r\n",       "id:",  raw->imu[0].id);
        CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "acc(G):",        raw->imu[0].acc[0], raw->imu[0].acc[1],  raw->imu[0].acc[2]);
        CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "gyr(deg/s):",    raw->imu[0].gyr[0], raw->imu[0].gyr[1],  raw->imu[0].gyr[2]);
        CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "mag(uT):",       raw->imu[0].mag[0], raw->imu[0].mag[1],  raw->imu[0].mag[2]);
        CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "eul(deg):",      raw->imu[0].eul[0], raw->imu[0].eul[1],  raw->imu[0].eul[2]);
        CH_TRACE("%-16s%.3f %.3f %.3f %.3f\r\n",  "quat:",          raw->imu[0].quat[0], raw->imu[0].quat[1],  raw->imu[0].quat[2], raw->imu[0].quat[3]);
        CH_TRACE("%-16s%.3f\r\n",       "presure(pa):",  raw->imu[0].pressure);
        CH_TRACE("%-16s%d\r\n",       "timestamp(ms):",  raw->imu[0].timestamp);

        CH_TRACE("item: ");
        for(i=0; i<raw->nitem_code; i++)
        {
            CH_TRACE("0x%02X ", raw->item_code[i]);
        }
        CH_TRACE("\r\n");
    }
    else
    {
        CH_TRACE("gateway: %s%d, %s%d\r\n",       "gwid:",      raw->gwid, "node cnt:", raw->nimu);
        for(i=0; i<raw->nimu; i++)
        {
            CH_TRACE("%-16s%d\r\n",       "id:",  raw->imu[i].id);
            CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "acc(G):",        raw->imu[i].acc[0], raw->imu[i].acc[1],  raw->imu[i].acc[2]);
            CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "gyr(deg/s):",    raw->imu[i].gyr[0], raw->imu[i].gyr[1],  raw->imu[i].gyr[2]);
            CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "mag(uT):",       raw->imu[i].mag[0], raw->imu[i].mag[1],  raw->imu[i].mag[2]);
            CH_TRACE("%-16s%.3f %.3f %.3f\r\n",       "eul(deg):",      raw->imu[i].eul[0], raw->imu[i].eul[1],  raw->imu[i].eul[2]);
            CH_TRACE("%-16s%.3f %.3f %.3f %.3f\r\n",  "quat:",          raw->imu[i].quat[0], raw->imu[i].quat[1],  raw->imu[i].quat[2], raw->imu[i].quat[3]);
            CH_TRACE("%-16s%.3f\r\n",       "presure(pa):",  raw->imu[i].pressure);
            CH_TRACE("%-16s%d\r\n",       "timestamp(ms):",  raw->imu[i].timestamp);
        }
    }
}


/**
 * @brief CRC16-CCITT校验
 * @param currect_crc previous crc value, set 0 if it's first section
 * @param src   source stream data
 * @param len
 * */
static void crc16_update(uint16_t *currect_crc, const uint8_t *src, uint32_t len)
{
    uint32_t crc = *currect_crc;
//    uint32_t j;
    for (uint32_t j=0; j < len; ++j)
    {
        uint32_t i;
        uint32_t byte = src[j];
        crc ^= byte << 8;   //异或
        for (i = 0; i < 8; ++i)
        {
            uint32_t temp = crc << 1;
            if (crc & 0x8000)
            {
                temp ^= 0x1021;
            }
            crc = temp;
        }
    } 
    *currect_crc = crc;
}


/**
 * @brief parse the payload of a frame and feed into data section (数据帧解析)
 * */
static int parse_data(raw_t *raw)
{
    int offset = 0, i = 0;
    uint8_t *p = &raw->buf[CH_HDR_SIZE];
    memset(raw->item_code, 0, sizeof(raw->item_code));
    raw->nitem_code = 0;

	while(offset < raw->len)
	{
        switch (raw->buf[1]) {
            case ID20:
                /// 对应8.8.1 System State Packet /系统状态数据包
                // 偏移照抄即可
                raw->nimu = 1;
                raw->item_code[raw->nitem_code++] = ID20;
                //TODO 还有4字节的系统、滤波器状态 和 4字节Unix时间秒 未读
                raw->imu[0].timestamp = U4(p + offset + 8);
                raw->imu[0].Latitude = R8(p+offset+12);
                raw->imu[0].Longitude = R8(p+offset+20);
                raw->imu[0].Height = R8(p+offset+28);
                raw->imu[0].v_world[0] = R4(p + offset + 36);
                raw->imu[0].v_world[1] = R4(p + offset + 40);
                raw->imu[0].v_world[2] = R4(p + offset + 44);
                raw->imu[0].acc[0] = R4(p + offset + 48);
                raw->imu[0].acc[1] = R4(p + offset + 52);
                raw->imu[0].acc[2] = R4(p + offset + 56);
                raw->imu[0].gravity = R4(p + offset + 60);
                raw->imu[0].eul[0] = R4(p + offset + 64);
                raw->imu[0].eul[1] = R4(p + offset + 68);
                raw->imu[0].eul[2] = R4(p + offset + 72);
                raw->imu[0].gyr[0] = R4(p + offset + 76);
                raw->imu[0].gyr[1] = R4(p + offset + 80);
                raw->imu[0].gyr[2] = R4(p + offset + 84);
                raw->imu[0].deviation[0] = R4(p + offset + 88);
                raw->imu[0].deviation[1] = R4(p + offset + 92);
                raw->imu[0].deviation[2] = R4(p + offset + 96);

                offset += 100;
                break;
//            // TODO 这部分有卫星信息
//            case ID31:
//                raw->nimu = 1;
//                raw->item_code[raw->nitem_code++] = ID31;
//                offset += 126;
//                break;
            case ID28:
                raw->nimu = 1;
                raw->item_code[raw->nitem_code++] = ID28;
                raw->imu[0].acc[0] = R4(p + offset) ;
                raw->imu[0].acc[1] = R4(p + offset + 4);
                raw->imu[0].acc[2] = R4(p + offset + 8);
                raw->imu[0].gyr[0] = R4(p + offset + 12);
                raw->imu[0].gyr[1] = R4(p + offset + 16);
                raw->imu[0].gyr[2] = R4(p + offset + 20);
                offset += 48;
                break;
            default:
                offset++;
                break;
        }
    }
    
    return 1;
}


/**
 * @brief 进行CRC校验
 * @param raw
 * */
static int decode_ch(raw_t *raw)
{
    uint16_t crc = 0xffff;
    uint8_t lrc = 0;

    /* checksum */
    lrc_check(&lrc, raw->buf+LRC_SIZE, CH_HDR_SIZE-LRC_SIZE);
    if(lrc != raw->buf[0]){
        CH_TRACE("LRC check error: frame:0x%X calcuate:0x%X, len:%d\n", raw->buf+LRC_SIZE, lrc, CH_HDR_SIZE-LRC_SIZE);
        return -1;
    }
    crc16_update(&crc, raw->buf+CH_HDR_SIZE, raw->len);
    if (crc != U2(raw->buf+3))
    {
        CH_TRACE("ch checksum error: frame:0x%X calcuate:0x%X, len:%d\n", U2(raw->buf+4), crc, raw->len);
        return -1;
    }

    return parse_data(raw);
}

/* sync code */
/*   同步码   */
static int sync_ch(uint8_t *buf, uint8_t data)
{
    buf[0] = buf[1];       // 对于NAV100，最终状态下 buf[0]应该是LRC校验位
    buf[1] = buf[2];
    buf[2] = data;
//    return (buf[1] == ID20 && buf[2] == LEN20) || (buf[1] == ID28 && buf[2] == LEN28)||(buf[1] == ID31 && buf[2] == LEN31);
    return (buf[1] == ID20 && buf[2] == LEN20) || (buf[1] == ID28 && buf[2] == LEN28);
//    return buf[1] == ID20 || buf[1] == ID28;
}

/**
 * @brief 找到数据包头，并解析整个数据包
 * @param raw 用于保存所读到的数据
 * @param data 读到的一个字节的数据
 * */
int ch_serial_input(raw_t *raw, uint8_t data)
{

    /* synchronize frame */
    /// 1、 如果是刚开始接收数据，则需要先找到数据包的数据头
    if (raw->nbyte == 0)
    {
        if (!sync_ch(raw->buf, data)){
//            memset(raw->item_code, 0, sizeof(raw->item_code));  // 清除上一个数据包的残留
            return 0;     // 寻找数据头
        }
        raw->nbyte = 3; //直到找到数据包的 1字节LRC校验位和2字节的帧头 之后才进行之后的操作
        return 0;
    }

    /// 2、找到数据头位置后开始读整个数据头（大小为CH_HDR_SIZE）
    raw->buf[raw->nbyte++] = data;
    if (raw->nbyte == CH_HDR_SIZE)  // 如果raw内数据足够一个数据头大小
    {
        raw->len = raw->buf[2];     // 数据头中显示的数据包长度
        if (raw->len  > (MAXRAWLEN - CH_HDR_SIZE))   // 判断数据域长度是否过大
        {
            CH_TRACE("ch length error: len=%d\n",raw->len);
            raw->nbyte = 0;  //数据域长度过大，可能数据有误，放弃该数据包
            return -1;
        }
    }

    ///  3、 数据头读完了，开等待数据包读完
    if (raw->nbyte < (raw->len + CH_HDR_SIZE)) 
    {
        return 0;
    }
    // 整个数据包读完之后，重置nbyte，便于读下一个数据包
    raw->nbyte = 0;

    /// 4、返回校验位是否正确
    return decode_ch(raw);
}
